/**
 * Copyright 2021 xpstem.com
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

package com.xpstem.lelink.device;

import com.xpstem.lelink.EventBus;
import com.xpstem.lelink.command.EV3Command;
import com.xpstem.lelink.command.EV3DirectCommand;
import com.xpstem.lelink.enums.*;
import com.xpstem.lelink.operation.EV3Operation;
import com.xpstem.lelink.operation.EV3OutputOpFactory;
import com.xpstem.lelink.type.Data32;
import com.xpstem.lelink.type.Data8;

import java.io.IOException;
import java.util.HashMap;
import java.util.Map;

/**
 * EV3 servo motor base class.
 *
 * @author billy zhang
 */
public abstract class EV3ServoMotor implements EV3OutputDevice {

    /**
     * MODE
     */
    public static final short MODE_DEGREE = 0;
    public static final short MODE_ROTATION = 1;
    public static final short MODE_POWER = 2;

    private EventBus eventBus;
    private OutputPort outPort;
    private String name;
    private short type;
    private short mode;
    private EV3MotorSensor sensor;

    private static Map<Short, String> modeMap;
    static {
        modeMap = new HashMap<>();
        modeMap.put(MODE_DEGREE, "Degree");
        modeMap.put(MODE_ROTATION, "Rotation");
        modeMap.put(MODE_POWER, "Power");
    }

    protected EV3ServoMotor(EventBus eventBus, OutputPort outPort, String name, short type, short mode) {
        this.eventBus = eventBus;
        this.outPort = outPort;
        this.name = name;
        this.type = type;
        this.mode = mode;

        if (outPort.getInputPort() != null) {
            sensor = new EV3MotorSensor(this, eventBus, outPort.getInputPort(), name, type, mode);
        }

    }

    @Override
    public OutputPort getOutPort() { return outPort; }

    @Override
    public String getName() { return name; }

    @Override
    public short getType() { return type; }

    @Override
    public short getMode() { return mode; }

    @Override
    public Map<Short, String> getModeMap() {
        return modeMap;
    }

    public EV3MotorSensor getSensor() {
        return sensor;
    }

    public void start() throws IOException {
        EV3Operation startOp = EV3OutputOpFactory.opOutputStart(
                eventBus.getLayer(),
                new OutputPort[] { outPort } );
        EV3Command cmd = new EV3DirectCommand(startOp);
        eventBus.postCommand(cmd);
    }

    public void stop(Brake brake) throws IOException {
        EV3Operation stopOp = EV3OutputOpFactory.opOutputStop(
                eventBus.getLayer(),
                new OutputPort[] { outPort },
                brake);
        EV3Command cmd = new EV3DirectCommand(stopOp);
        eventBus.postCommand(cmd);
    }

    @Override
    public boolean ready() throws IOException {
        EV3Operation op = EV3OutputOpFactory.opOutputReady(
                eventBus.getLayer(), new OutputPort[] {outPort} );
        EV3DirectCommand cmd = new EV3DirectCommand(op);
        eventBus.postCommand(cmd);
        return true;
    }

    /**
     *
     * @param power
     * @throws IOException
     */
    public void startOnPower(int power) throws IOException {
        EV3Operation powerOp = EV3OutputOpFactory.opOutputPower(
                eventBus.getLayer(),
                new OutputPort[] { outPort },
                Data8.from(Value.power(power)));
        EV3Operation startOp = EV3OutputOpFactory.opOutputStart(
                eventBus.getLayer(),
                new OutputPort[] { outPort } );
        EV3Command cmd = new EV3DirectCommand(powerOp, startOp);
        eventBus.postCommand(cmd);
    }

    /**
     * 运行指定圈数。
     * 圈数 = step1 + step2 + step3
     *
     * @param power 指定功率
     * @param degree1 从0加速到指定功能率运行圈数，决定启动是否平稳
     * @param degree2 按指定功率运行的圈数
     * @param degree3 从指定功率减速到0所运行圈数，决定停止是否平稳
     * @param brake
     * @throws IOException
     */
    public void startOnStepPower(int power, int degree1, int degree2, int degree3, Brake brake) throws IOException {
        EV3Operation powerOp = EV3OutputOpFactory.opOutputStepPower(
                eventBus.getLayer(),
                new OutputPort[] { outPort },
                Data8.from(Value.power(power)),
                Data32.from(degree1),
                Data32.from(degree2),
                Data32.from(degree3),
                brake);
        EV3Operation startOp = EV3OutputOpFactory.opOutputStart(
                eventBus.getLayer(),
                new OutputPort[] { outPort } );
        EV3Command cmd = new EV3DirectCommand(powerOp, startOp);
        eventBus.postCommand(cmd);
    }

    /**
     * 运行指定时长。
     * 时长 = time1 + time2 + time3
     *
     * @param power 指定功率
     * @param time1 从0加速到指定功能率运行时间，决定启动是否平稳
     * @param time2 按指定功率运行的时间
     * @param time3 从指定功率减速到0所运行时间，决定停止是否平稳
     * @param brake
     * @throws IOException
     */
    public void startOnTimePower(int power, int time1, int time2, int time3, Brake brake) throws IOException {
        EV3Operation powerOp = EV3OutputOpFactory.opOutputTimePower(
                eventBus.getLayer(),
                new OutputPort[] { outPort },
                Data8.from(Value.power(power)),
                Data32.from(time1),
                Data32.from(time2),
                Data32.from(time3),
                brake);
        EV3Operation startOp = EV3OutputOpFactory.opOutputStart(
                eventBus.getLayer(),
                new OutputPort[] { outPort } );
        EV3Command cmd = new EV3DirectCommand(powerOp, startOp);
        eventBus.postCommand(cmd);
    }

    public void startOnSpeed(int speed) throws IOException {
        EV3Operation speedOp = EV3OutputOpFactory.opOutputSpeed(
                eventBus.getLayer(),
                new OutputPort[] { outPort },
                Data8.from(Value.speed(speed)));
        EV3Operation startOp = EV3OutputOpFactory.opOutputStart(
                eventBus.getLayer(),
                new OutputPort[] { outPort } );
        EV3Command cmd = new EV3DirectCommand(speedOp, startOp);
        eventBus.postCommand(cmd);
    }

    /**
     * 运行指定圈数。
     * 圈数 = step1 + step2 + step3
     *
     * @param speed 指定速度
     * @param degree1 从0加速到指定功能率运行圈数，决定启动是否平稳
     * @param degree2 按指定功率运行的圈数
     * @param degree3 从指定功率减速到0所运行圈数，决定停止是否平稳
     * @param brake
     * @throws IOException
     */
    public void startOnStepSpeed(int speed, int degree1, int degree2, int degree3, Brake brake) throws IOException {

        EV3Operation speedOp = EV3OutputOpFactory.opOutputStepSpeed(
                eventBus.getLayer(),
                new OutputPort[] { outPort },
                Data8.from(Value.speed(speed)),
                Data32.from(degree1),
                Data32.from(degree2),
                Data32.from(degree3),
                brake);
        EV3Operation startOp = EV3OutputOpFactory.opOutputStart(
                eventBus.getLayer(),
                new OutputPort[] { outPort } );
        EV3Command cmd = new EV3DirectCommand(speedOp, startOp);
        eventBus.postCommand(cmd);
    }

    /**
     * 运行指定时长。
     * 时长 = time1 + time2 + time3
     *
     * @param speed 指定功率
     * @param time1 从0加速到指定功能率运行时间，决定启动是否平稳
     * @param time2 按指定功率运行的时间
     * @param time3 从指定功率减速到0所运行时间，决定停止是否平稳
     * @param brake
     * @throws IOException
     */
    public void startOnTimeSpeed(int speed, int time1, int time2, int time3, Brake brake) throws IOException {

        EV3Operation speedOp = EV3OutputOpFactory.opOutputTimeSpeed(
                eventBus.getLayer(),
                new OutputPort[] { outPort },
                Data8.from(Value.speed(speed)),
                Data32.from(time1),
                Data32.from(time2),
                Data32.from(time3),
                brake);
        EV3Operation startOp = EV3OutputOpFactory.opOutputStart(
                eventBus.getLayer(),
                new OutputPort[] { outPort } );
        EV3Command cmd = new EV3DirectCommand(speedOp, startOp);
        eventBus.postCommand(cmd);
    }

}
